/**
 *  @file ToneDetector.cpp
 *
 *  @date 16.04.2014
 *  @author Armin Joachimsmeyer
 *  Email: armin.joachimsmeyer@gmail.com
 *  License: GPL v3 (http://www.gnu.org/licenses/gpl.html)
 *  Sources: https://code.google.com/p/arduino-tone-detector/
 *
 *  @version 1.0.3
 *
 *  @brief analyzes a microphone signal and outputs the detected frequency.
 *  computeMatch() compares the frequency with min and values and low pass filters the result
 *
 */

#if defined (__AVR_ATmega328P__) || defined (__AVR_ATmega328__)
//#define DEBUG
//#define TRACE
#endif

#include <Arduino.h>
#include "toneDetector.h"

// Number of samples used for detecting the period of the signal -> 26.624 milliseconds 37,560 Hz
#define NUMBER_OF_SAMPLES 512

/*
 * plausibility for signal/frequency
 */
#define RAW_VOLTAGE_MIN_DELTA_DEFAULT 0x40
#define PERIOD_COUNT_MIN_DEFAULT 16  // 601 Hz
#define PERIOD_COUNT_MAX_DEFAULT (SIZE_OF_PERIOD_LENGTH_ARRAY_FOR_PLAUSI - 1)  // 2403 Hz
#define MAX_DROPOUT_COUNT_BEFORE_NO_FILTERED_MATCH_DEFAULT 10 // number of allowed (Frequency10Actual == 0) conditions before filtered match goes Invalid
/*
 * values for low pass filtering the match result
 */
#define FILTER_VALUE_HIGHER  0x200
#define FILTER_VALUE_LOWER   0
#define FILTER_VALUE_MATCH   ((FILTER_VALUE_HIGHER + FILTER_VALUE_LOWER)/2)
#define FILTER_VALUE_MATCH_UPPER_THRESHOLD ((FILTER_VALUE_HIGHER + FILTER_VALUE_MATCH)/2)
#define FILTER_VALUE_MATCH_LOWER_THRESHOLD ((FILTER_VALUE_LOWER + FILTER_VALUE_MATCH)/2)

ToneDetectorControlStruct ToneDetectorControl;

// Union to speed up the combination of low and high bytes to a word
// it is not optimal since the compiler still generates 2 unnecessary moves
// but using  -- value = (high << 8) | low -- gives 5 unnecessary instructions
union Myword {
    struct {
        uint8_t LowByte;
        uint8_t HighByte;
    } byte;
    uint16_t Word;
    uint8_t * BytePointer;
};

void setToneDetectorControlDefaults(void) {
    ToneDetectorControl.RawVoltageMinDelta = RAW_VOLTAGE_MIN_DELTA_DEFAULT;
    ToneDetectorControl.PeriodCountMin = PERIOD_COUNT_MIN_DEFAULT;
    ToneDetectorControl.PeriodCountMax = PERIOD_COUNT_MAX_DEFAULT;
    ToneDetectorControl.MaxMatchDropoutCount = MAX_DROPOUT_COUNT_BEFORE_NO_FILTERED_MATCH_DEFAULT;
}

/*
 * ADC read routine reads 512 samples and computes:
 * Frequency / Period of signal
 * Triggering value for next reading, trigger hysteresis is 1/8 peak to peak value
 */
void analyzeSignal(uint8_t aPrescaleRegisterValue) {
    /**********************************
     * wait for triggering condition
     **********************************/
    Myword tUValue;
    bool tTriggerSearchStart = true;
    bool tSignalTriggerFound = false;
    uint16_t i;
    uint16_t tValueMax;
    uint16_t tValueMin;
    uint16_t tPeriodCountPosition = 0;
    uint8_t tPeriodCountActual;

    /*
     * Read 513 samples
     */
//  ADCSRB = 0; // free running mode  - is default
    ADCSRA = ((1 << ADEN) | (1 << ADSC) | (1 << ADATE) | (1 << ADIF) | aPrescaleRegisterValue);

// wait for free running conversion to finish
    while (bit_is_clear(ADCSRA, ADIF))
        ;
    tUValue.byte.LowByte = ADCL;
    tUValue.byte.HighByte = ADCH;

// without "| (1 << ADSC)" it does not work - undocumented feature???
    ADCSRA |= (1 << ADIF) | (1 << ADSC);

    // Initialize max and min with first value
    tValueMax = tUValue.Word;
    tValueMin = tUValue.Word;

    //digitalToggleFast(LED_DEBUG);
    tPeriodCountActual = 0;

    for (i = 0; i < NUMBER_OF_SAMPLES; i++) {
        // wait for free running conversion to finish
        while (bit_is_clear(ADCSRA, ADIF))
            ;
        // Get value
        tUValue.byte.LowByte = ADCL;
        tUValue.byte.HighByte = ADCH;
        // without "| (1 << ADSC)" it does not work - undocumented feature???
        ADCSRA |= (1 << ADIF) | (1 << ADSC); // clear bit to recognize next conversion has finished

        /*
         * Detect trigger
         */
        if (tTriggerSearchStart) {
            // rising slope - wait for value below 1. threshold
            if (tUValue.Word < ToneDetectorControl.TriggerLevelLower) {
                tTriggerSearchStart = false;
            }
        } else {
            // rising slope - wait for value to rise above 2. threshold
            if (tUValue.Word > ToneDetectorControl.TriggerLevelUpper) {
                //Trigger found but skip first (incomplete period)
                if (tSignalTriggerFound) {
                    ToneDetectorControl.PeriodLength[tPeriodCountActual] = i - tPeriodCountPosition;
                    if (tPeriodCountActual < (sizeof(ToneDetectorControl.PeriodLength) - 1)) {
                        tPeriodCountActual++;
                    }
                    tPeriodCountPosition = i;
                } else {
                    ToneDetectorControl.PeriodCountFirstPosition = i;
                    tPeriodCountPosition = i;
                }
                tSignalTriggerFound = true;
                tTriggerSearchStart = true;
            }
        }

        // get max and min for automatic triggering - needs 0,4 microseconds
        if (tUValue.Word > tValueMax) {
            tValueMax = tUValue.Word;
        } else if (tUValue.Word < tValueMin) {
            tValueMin = tUValue.Word;
        }

    }
    ADCSRA &= ~(1 << ADATE); // Disable auto-triggering

    ToneDetectorControl.PeriodCountLastPosition = tPeriodCountPosition;
    ToneDetectorControl.PeriodCountActual = tPeriodCountActual;
    /*
     * check for signal strength
     */
    uint16_t tDelta = tValueMax - tValueMin;
    if (tDelta < ToneDetectorControl.RawVoltageMinDelta) {
        ToneDetectorControl.Frequency10HzActual = SIGNAL_STRENGTH_LOW;
        return; // don't compute new trigger value because signal is too low!!!!
    }

// middle between min and max
    uint16_t tTriggerValue = tValueMin + (tDelta / 2);
    // set hysteresis
    uint8_t TriggerHysteresis = tDelta / 8;
    ToneDetectorControl.TriggerLevelUpper = tTriggerValue;
    ToneDetectorControl.TriggerLevelLower = tTriggerValue - TriggerHysteresis;

#ifdef TRACE
    Serial.print("Delta U=");
    Serial.print(tDelta);
    Serial.print(" TriggerValue=");
    Serial.print(tTriggerValue);
    Serial.print(" PeriodCount=");
    Serial.print(ToneDetectorControl.PeriodCountActual);
    Serial.print(" samples=");
    Serial.print(ToneDetectorControl.PeriodCountLastPosition - ToneDetectorControl.PeriodCountFirstPosition);
    Serial.print(" Freq=");
    Serial.println(ToneDetectorControl.Frequency10HzActual);
#endif
    return;
}

/*
 * Used plausi is:
 * A trigger must be detected in first and last 1/8 of samples
 * Only 1/8 of the samples are allowed to be greater than 1.5 or less than 0.75 of the average period
 */
void doPlausi(void) {
    uint8_t tPeriodCountActual = ToneDetectorControl.PeriodCountActual;
    /*
     * plausibility check
     */
    if (tPeriodCountActual >= ToneDetectorControl.PeriodCountMin
            && tPeriodCountActual <= ToneDetectorControl.PeriodCountMax
            && ToneDetectorControl.PeriodCountFirstPosition < (NUMBER_OF_SAMPLES / 8)
            && ToneDetectorControl.PeriodCountLastPosition > NUMBER_OF_SAMPLES - (NUMBER_OF_SAMPLES / 8)) {
        /*
         * check if not more than 1/8 of periods are out of range - less than 0.75 or more than 1.5
         */
        // average can be between 8 an 32
        uint8_t tAveragePeriod = (ToneDetectorControl.PeriodCountLastPosition
                - ToneDetectorControl.PeriodCountFirstPosition) / tPeriodCountActual;
        uint8_t tPeriodMax = tAveragePeriod + (tAveragePeriod / 2);
        uint8_t tPeriodMin = tAveragePeriod - (tAveragePeriod / 4);
        uint8_t tErrorCount = 0;
#ifdef TRACE
        Serial.print(tAveragePeriod);
        Serial.print("  ");
#endif
        for (uint8_t i = 0; i < tPeriodCountActual; ++i) {
#ifdef TRACE
            Serial.print(ToneDetectorControl.PeriodLength[i]);
            Serial.print(",");
#endif
            if (ToneDetectorControl.PeriodLength[i] > tPeriodMax || ToneDetectorControl.PeriodLength[i] < tPeriodMin) {
                tErrorCount++;
            }
        }
        if (tErrorCount > tPeriodCountActual / 8) {
            ToneDetectorControl.Frequency10HzActual = SIGNAL_DISTRIBUTION_PLAUSI_FAILED;
        } else {
            ToneDetectorControl.Frequency10HzActual = (tPeriodCountActual * 1923L)
                    / (ToneDetectorControl.PeriodCountLastPosition - ToneDetectorControl.PeriodCountFirstPosition);
        }
#ifdef TRACE
        Serial.print(tErrorCount);
        Serial.print("  #=");
        Serial.print(tPeriodCountActual);
        Serial.print("  F=");
        Serial.print(ToneDetectorControl.Frequency10HzActual);
        Serial.println("0Hz");

#endif
    } else {
        ToneDetectorControl.Frequency10HzActual = SIGNAL_FIRST_PLAUSI_FAILED;
    }
}

/**
 * simple low-pass filter
 * history of 15 values
 */
uint16_t filter(uint16_t aFilteredValue, uint16_t aValue) {
    uint16_t tValue = (aFilteredValue << 4) - aFilteredValue; // aFilteredValue * 15
    tValue += aValue + (1 << 3); // to avoid rounding errors
    return (tValue >> 4); // tValue/16
}

/**
 * handles dropouts / no signal
 * determine match state - ToneDetectorControl.ToneMatch
 * computes new low-pass filtered state - ToneDetectorControl.ToneMatchFiltered
 */
void computeMatch(void) {
    uint16_t tFrequency10Hz = ToneDetectorControl.Frequency10HzActual;

    /*
     *  dropout / no signal handling
     */
    if (tFrequency10Hz <= SIGNAL_MAX_ERROR_CODE) {
        ToneDetectorControl.ToneMatch = TONE_MATCH_INVALID;
        ToneDetectorControl.MatchDropoutCount++;
        if (ToneDetectorControl.MatchDropoutCount >= ToneDetectorControl.MaxMatchDropoutCount) {
            // clip value and set state to off
            ToneDetectorControl.MatchDropoutCount = ToneDetectorControl.MaxMatchDropoutCount;
            ToneDetectorControl.ToneMatchFiltered = TONE_MATCH_INVALID;
        }
    } else {
        /*
         * Determine match state
         */
        uint16_t tNewFilterValue;
        if (tFrequency10Hz < ToneDetectorControl.Frequency10HzMin) {
            // Tone too low
            ToneDetectorControl.ToneMatch = TONE_MATCH_LOWER;
            tNewFilterValue = FILTER_VALUE_LOWER;
        } else if (tFrequency10Hz > ToneDetectorControl.Frequency10HzMax) {
            // Tone too high
            ToneDetectorControl.ToneMatch = TONE_MATCH_HIGHER;
            tNewFilterValue = FILTER_VALUE_HIGHER;
        } else {
            // Tone matches
            ToneDetectorControl.ToneMatch = TONE_MATCH;
            tNewFilterValue = FILTER_VALUE_MATCH;
        }

        /*
         * Low pass filter frequency and match (lower, match, greater)
         */
        if (ToneDetectorControl.MatchDropoutCount == ToneDetectorControl.MaxMatchDropoutCount) {
            // init filter at first time after no signal
            ToneDetectorControl.MatchLowPassFiltered = tNewFilterValue;
            ToneDetectorControl.Frequency10HzFiltered = tFrequency10Hz;
        } else {
            ToneDetectorControl.Frequency10HzFiltered = filter(ToneDetectorControl.Frequency10HzFiltered,
                    tFrequency10Hz);
            ToneDetectorControl.MatchLowPassFiltered = filter(ToneDetectorControl.MatchLowPassFiltered,
                    tNewFilterValue);
        }
        // decrement dropout count until 0 and increment valid periods count
        if (ToneDetectorControl.MatchDropoutCount > 0) {
            ToneDetectorControl.MatchDropoutCount--;
        }

        /*
         * determine new low pass filtered state
         */
        if (ToneDetectorControl.MatchDropoutCount >= ToneDetectorControl.MaxMatchDropoutCount) {
            ToneDetectorControl.ToneMatchFiltered = TONE_MATCH_INVALID;
        } else {
            if (ToneDetectorControl.MatchLowPassFiltered > FILTER_VALUE_MATCH_UPPER_THRESHOLD) {
                ToneDetectorControl.ToneMatchFiltered = TONE_MATCH_HIGHER;
            } else if (ToneDetectorControl.MatchLowPassFiltered < FILTER_VALUE_MATCH_LOWER_THRESHOLD) {
                ToneDetectorControl.ToneMatchFiltered = TONE_MATCH_LOWER;
            } else {
                ToneDetectorControl.ToneMatchFiltered = TONE_MATCH;
            }
        }
    }
}

